/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.kreig133.spine.algorithm;

import com.kreig133.spine.abstractmodel.AbstractEnvironment;
import com.kreig133.spine.abstractmodel.AbstractAlgorithm;
import com.kreig133.spine.abstractmodel.AbstractState;
import com.kreig133.spine.simplemodel.SimpleEnvironment;
import com.kreig133.spine.simplemodel.SimpleState;
import static java.lang.Math.*;

/**
 *
 * @author C.C.-fag
 */
public class AlgorithmTangent extends AbstractAlgorithm{

    @Override
   public AbstractState calculate(AbstractEnvironment env, AbstractState state) {

        SimpleState next = (SimpleState) ((SimpleEnvironment) env).getNext();
        SimpleState previous = (SimpleState) ((SimpleEnvironment) env).getPrevious();

        double downForce = 0.0;
        if (next != null) {
            downForce = - calculateFiTg(6000.0, PI, -0.5,
                    next.getCoordinate()- (  (SimpleState) state).getCoordinate() );
        }
        SimpleState simpleState = (SimpleState) state;
        double deltaT = ((SimpleEnvironment) env).getSimulationStep();
 
//        System.out.println(simpleState.getCoordinate());
//        System.out.println(simpleState.getSpeed());
//        System.out.println(deltaT);
//        System.out.println(downForce);
        
        double x = simpleState.getCoordinate() + simpleState.getSpeed() * deltaT +
                deltaT * deltaT * ( previous.getUpForce()-downForce );

        if (x < previous.getCoordinate()) {
            System.out.println("Настрой логи уже, тут косяки есть!");
            x = previous.getCoordinate()+0.00001;
        }
//        System.out.println("------------------------------------");
//        System.out.println(Thread.currentThread().getName()+" x =   "+x);
//        System.out.println("Previous str = "+previous.getUpForce());
//        System.out.println("downForce = "+downForce);
        return new SimpleState(x, downForce);
    }

    public double calculateFiTg(double a, double b, double c, double y) {
        return a * tan(b * (y + c));
    }
}
